/**
 * @author Russell Toris - rctoris@wpi.edu
 * @author Lars Kunze - l.kunze@cs.bham.ac.uk
 */

var NAV2D = NAV2D || {
  REVISION: "0.3.0"
};

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator.
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * topic (optional) - the map meta data topic to listen to
 *   * image - the URL of the image to render
 *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
 *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
 *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
 *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
 *   * viewer - the main viewer to render to
 */
NAV2D.ImageMapClientNav = function (options) {
  var that = this;
  options = options || {};
  this.ros = options.ros;
  var topic = options.topic || "/map_metadata";
  var image = options.image;
  this.serverName = options.serverName || "/move_base";
  this.actionName = options.actionName || "move_base_msgs/MoveBaseAction";
  this.rootObject = options.rootObject || new createjs.Container();
  this.viewer = options.viewer;
  this.withOrientation = options.withOrientation || false;

  this.navigator = null;

  // setup a client to get the map
  var client = new ROS2D.ImageMapClient({
    ros: this.ros,
    rootObject: this.rootObject,
    topic: topic,
    image: image
  });
  client.on("change", function () {
    that.navigator = new NAV2D.Navigator({
      ros: that.ros,
      serverName: that.serverName,
      actionName: that.actionName,
      rootObject: that.rootObject,
      withOrientation: that.withOrientation
    });

    // scale the viewer to fit the map
    that.viewer.scaleToDimensions(
      client.currentImage.width,
      client.currentImage.height
    );
    that.viewer.shift(
      client.currentImage.pose.position.x,
      client.currentImage.pose.position.y
    );
  });
};

/**
 * @author Russell Toris - rctoris@wpi.edu
 * @author Lars Kunze - l.kunze@cs.bham.ac.uk
 */

/**
 * A navigator can be used to add click-to-navigate options to an object. If
 * withOrientation is set to true, the user can also specify the orientation of
 * the robot by clicking at the goal position and pointing into the desired
 * direction (while holding the button pressed).
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
 *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
 *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
 *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
 */
NAV2D.Navigator = function (options) {
  var that = this;
  options = options || {};
  var ros = options.ros;
  var serverName = options.serverName || "/move_base";
  var actionName = options.actionName || "move_base_msgs/MoveBaseAction";
  var withOrientation = options.withOrientation || false;
  var positions = options.positions || [];
  this.rootObject = options.rootObject || new createjs.Container();

  // setup the actionlib client
  var actionClient = new ROSLIB.ActionClient({
    ros: ros,
    actionName: actionName,
    serverName: serverName
  });

  var navigationClient = new ROSLIB.Topic({
    ros: ros,
    name: "/move_base_simple/goal",
    messageType: "geometry_msgs/PoseStamped"
  });
  var globalPath = new ROSLIB.Topic({
    ros: ros,
    name: "/move_base/GlobalPlanner/plan",
    messageType: "nav_msgs/Path"
  });
  var localPath = new ROSLIB.Topic({
    ros: ros,
    name: "/move_base/TebLocalPlannerROS/local_plan",
    messageType: "nav_msgs/Path"
  });
  globalPath.subscribe(function (message) {
    var pathShape = new ROS2D.PathShape({
      path: message,
      strokeSize: 0.1,
      strokeColor: createjs.Graphics.getRGB(25, 255, 0)
    });
    that.rootObject.addChild(pathShape);
  });

  localPath.subscribe(function (message) {
    var pathShape = new ROS2D.PathShape({
      path: message,
      strokeSize: 0.1,
      strokeColor: createjs.Graphics.getRGB(239, 41, 41)
    });
    that.rootObject.addChild(pathShape);
  });

  poseTopic = new ROSLIB.Topic({
    ros: ros,
    name: "/amcl_pose",
    messageType: "geometry_msgs/PoseWithCovarianceStamped"
  });

  let currentGoalIndex = 0;

  poseTopic.subscribe(function (message) {
    var robotPose = new ROSLIB.Pose({
      position: new ROSLIB.Vector3(message.pose.pose.position),
      orientation: new ROSLIB.Quaternion(message.pose.pose.orientation)
    });
  
    // 检查机器人是否已经到达目标点
    if (currentGoalIndex > 0 && currentGoalIndex <= positions.length) {
      const previousGoalPosition = positions[currentGoalIndex - 1];
      const distance = Math.sqrt(
        Math.pow(robotPose.position.x - previousGoalPosition.x, 2) +
          Math.pow(robotPose.position.y - previousGoalPosition.y, 2)
      );
      // console.log("Distance to previous goal:", distance);
      // 如果机器人已经到达目标点（或者非常接近目标点），那么发送下一个目标点
      if (distance < 0.2) {
        sendNextGoal();
        console.log("Goal reached, sending next goal");
      }
    }
  });

  function sendNextGoal() {
    if (currentGoalIndex < positions.length) {
      var nextGoal_msg = new ROSLIB.Message({
        header: {
          frame_id: "map"
        },
        pose: {
          position: new ROSLIB.Vector3(positions[currentGoalIndex]),
          orientation: new ROSLIB.Quaternion({
            x: 0,
            y: 0,
            z: 0,
            w: 1
          })
        }
      });
      sendGoal(nextGoal_msg);
      currentGoalIndex++;
    }
  }

  /**
   * Send a goal to the navigation stack with the given pose.
   *
   * @param pose - the goal pose
   */
  function sendGoal(pose) {
    // create a goal
    var goal = new ROSLIB.Goal({
      actionClient: actionClient,
      goalMessage: {
        target_pose: {
          header: {
            frame_id: "/map"
          },
          pose: pose
        }
      }
    });
    goal.send();
    navigationClient.publish(pose);
    // create a marker for the goal
    var goalMarker = new ROS2D.NavigationArrow({
      size: 15,
      strokeSize: 1,
      fillColor: createjs.Graphics.getRGB(255, 64, 128, 0.66),
      pulse: true
    });
    goalMarker.x = pose.pose.position.x;
    goalMarker.y = -pose.pose.position.y;
    goalMarker.rotation = stage.rosQuaternionToGlobalTheta(
      pose.pose.orientation
    );
    goalMarker.scaleX = 1.0 / stage.scaleX;
    goalMarker.scaleY = 1.0 / stage.scaleY;
    that.rootObject.addChild(goalMarker);

    goal.on("result", function () {
      that.rootObject.removeChild(goalMarker);
    });
  }

  // get a handle to the stage
  var stage;
  if (that.rootObject instanceof createjs.Stage) {
    stage = that.rootObject;
  } else {
    stage = that.rootObject.getStage();
  }

  // marker for the robot
  var robotMarker = new ROS2D.NavigationArrow({
    size: 25,
    strokeSize: 1,
    fillColor: createjs.Graphics.getRGB(255, 128, 0, 0.66),
    pulse: true
  });
  // wait for a pose to come in first
  robotMarker.visible = false;
  this.rootObject.addChild(robotMarker);
  var initScaleSet = false;

  // setup a listener for the robot pose
  var poseListener = new ROSLIB.Topic({
    ros: ros,
    name: "/amcl_pose",
    messageType: "geometry_msgs/PoseWithCovarianceStamped",
    throttle_rate: 100
  });
  poseListener.subscribe(function (pose) {
    // update the robots position on the map
    robotMarker.x = pose.pose.pose.position.x;
    robotMarker.y = -pose.pose.pose.position.y;
    if (!initScaleSet) {
      robotMarker.scaleX = 1.0 / stage.scaleX;
      robotMarker.scaleY = 1.0 / stage.scaleY;
      initScaleSet = true;
    }

    // change the angle
    robotMarker.rotation = stage.rosQuaternionToGlobalTheta(
      pose.pose.pose.orientation
    );

    robotMarker.visible = true;
  });

  if (withOrientation === false) {
    if (positions.length) {
      sendNextGoal();
    } else {
      // setup a double click listener (no orientation)
      this.rootObject.addEventListener("dblclick", function (event) {
        // convert to ROS coordinates
        var coords = stage.globalToRos(event.stageX, event.stageY);
        // var pose = new ROSLIB.PoseStamped({
        //   position: new ROSLIB.Vector3(coords)
        // });
        // send the goal
        var goal_msg = new ROSLIB.Message({
          header: {
            frame_id: "map"
          },
          pose: {
            position: new ROSLIB.Vector3(coords),
            orientation: new ROSLIB.Quaternion({
              x: 0,
              y: 0,
              z: 0,
              w: 1
            })
          }
        });
        sendGoal(goal_msg);
      });
    }
  } else {
    // withOrientation === true
    // setup a click-and-point listener (with orientation)
    var position = null;
    var positionVec3 = null;
    var thetaRadians = 0;
    var thetaDegrees = 0;
    var orientationMarker = null;
    var mouseDown = false;
    var xDelta = 0;
    var yDelta = 0;

    var mouseEventHandler = function (event, mouseState) {
      if (mouseState === "down") {
        // get position when mouse button is pressed down
        position = stage.globalToRos(event.stageX, event.stageY);
        positionVec3 = new ROSLIB.Vector3(position);
        mouseDown = true;
      } else if (mouseState === "move") {
        // remove obsolete orientation marker
        that.rootObject.removeChild(orientationMarker);

        if (mouseDown === true) {
          // if mouse button is held down:
          // - get current mouse position
          // - calulate direction between stored <position> and current position
          // - place orientation marker
          var currentPos = stage.globalToRos(event.stageX, event.stageY);
          var currentPosVec3 = new ROSLIB.Vector3(currentPos);

          orientationMarker = new ROS2D.NavigationArrow({
            size: 25,
            strokeSize: 1,
            fillColor: createjs.Graphics.getRGB(0, 255, 0, 0.66),
            pulse: false
          });

          xDelta = currentPosVec3.x - positionVec3.x;
          yDelta = currentPosVec3.y - positionVec3.y;

          thetaRadians = Math.atan2(xDelta, yDelta);

          thetaDegrees = thetaRadians * (180.0 / Math.PI);

          if (thetaDegrees >= 0 && thetaDegrees <= 180) {
            thetaDegrees += 270;
          } else {
            thetaDegrees -= 90;
          }

          orientationMarker.x = positionVec3.x;
          orientationMarker.y = -positionVec3.y;
          orientationMarker.rotation = thetaDegrees;
          orientationMarker.scaleX = 1.0 / stage.scaleX;
          orientationMarker.scaleY = 1.0 / stage.scaleY;

          that.rootObject.addChild(orientationMarker);
        }
      } else if (mouseDown) {
        // mouseState === 'up'
        // if mouse button is released
        // - get current mouse position (goalPos)
        // - calulate direction between stored <position> and goal position
        // - set pose with orientation
        // - send goal
        mouseDown = false;

        var goalPos = stage.globalToRos(event.stageX, event.stageY);

        var goalPosVec3 = new ROSLIB.Vector3(goalPos);

        xDelta = goalPosVec3.x - positionVec3.x;
        yDelta = goalPosVec3.y - positionVec3.y;

        thetaRadians = Math.atan2(xDelta, yDelta);

        if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
          thetaRadians += (3 * Math.PI) / 2;
        } else {
          thetaRadians -= Math.PI / 2;
        }

        var qz = Math.sin(-thetaRadians / 2.0);
        var qw = Math.cos(-thetaRadians / 2.0);

        var orientation = new ROSLIB.Quaternion({ x: 0, y: 0, z: qz, w: qw });

        // var pose = new ROSLIB.Pose({
        //   position: positionVec3,
        //   orientation: orientation
        // });
        var goal_msg = new ROSLIB.Message({
          header: {
            frame_id: "map"
          },
          pose: {
            position: positionVec3,
            orientation: orientation
          }
        });
        // send the goal
        sendGoal(goal_msg);
        console.log("Goal sent");
      }
    };

    this.rootObject.addEventListener("stagemousedown", function (event) {
      mouseEventHandler(event, "down");
    });

    this.rootObject.addEventListener("stagemousemove", function (event) {
      mouseEventHandler(event, "move");
    });

    this.rootObject.addEventListener("stagemouseup", function (event) {
      mouseEventHandler(event, "up");
    });
  }
};

NAV2D.InitialPose = function (options) {
  var that = this;
  options = options || {};
  var ros = options.ros;
  this.rootObject = options.rootObject || new createjs.Container();
  var isPub = false;
  // get a handle to the stage
  var stage;
  if (that.rootObject instanceof createjs.Stage) {
    stage = that.rootObject;
  } else {
    stage = that.rootObject.getStage();
  }

  // setup a click-and-point listener (with orientation)
  var position = null;
  var positionVec3 = null;
  var thetaRadians = 0;
  var thetaDegrees = 0;
  var orientationMarker = null;
  var mouseDown = false;
  var xDelta = 0;
  var yDelta = 0;

  var InitialPoseClient = new ROSLIB.Topic({
    ros: ros,
    name: "/initialpose",
    messageType: "geometry_msgs/PoseWithCovarianceStamped"
  });

  var mouseEventHandler = function (event, mouseState) {
    if (mouseState === "down") {
      // get position when mouse button is pressed down
      position = stage.globalToRos(event.stageX, event.stageY);
      positionVec3 = new ROSLIB.Vector3(position);
      mouseDown = true;
    } else if (mouseState === "move") {
      // remove obsolete orientation marker
      that.rootObject.removeChild(orientationMarker);

      if (mouseDown === true) {
        // if mouse button is held down:
        // - get current mouse position
        // - calulate direction between stored <position> and current position
        // - place orientation marker
        var currentPos = stage.globalToRos(event.stageX, event.stageY);
        var currentPosVec3 = new ROSLIB.Vector3(currentPos);

        orientationMarker = new ROS2D.NavigationArrow({
          size: 25,
          strokeSize: 1,
          fillColor: createjs.Graphics.getRGB(0, 255, 0, 0.66),
          pulse: false
        });

        xDelta = currentPosVec3.x - positionVec3.x;
        yDelta = currentPosVec3.y - positionVec3.y;

        thetaRadians = Math.atan2(xDelta, yDelta);

        thetaDegrees = thetaRadians * (180.0 / Math.PI);

        if (thetaDegrees >= 0 && thetaDegrees <= 180) {
          thetaDegrees += 270;
        } else {
          thetaDegrees -= 90;
        }

        orientationMarker.x = positionVec3.x;
        orientationMarker.y = -positionVec3.y;
        orientationMarker.rotation = thetaDegrees;
        orientationMarker.scaleX = 1.0 / stage.scaleX;
        orientationMarker.scaleY = 1.0 / stage.scaleY;

        that.rootObject.addChild(orientationMarker);
      }
    } else if (mouseDown) {
      mouseDown = false;

      var goalPos = stage.globalToRos(event.stageX, event.stageY);
      console.log("Initial Pose goal:", event.stageX, event.stageY);
      var goalPosVec3 = new ROSLIB.Vector3(goalPos);

      xDelta = goalPosVec3.x - positionVec3.x;
      yDelta = goalPosVec3.y - positionVec3.y;

      thetaRadians = Math.atan2(xDelta, yDelta);

      if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
        thetaRadians += (3 * Math.PI) / 2;
      } else {
        thetaRadians -= Math.PI / 2;
      }

      var qz = Math.sin(-thetaRadians / 2.0);
      var qw = Math.cos(-thetaRadians / 2.0);

      var orientation = new ROSLIB.Quaternion({ x: 0, y: 0, z: qz, w: qw });

      var pose = new ROSLIB.Pose({
        position: positionVec3,
        orientation: orientation
      });
      var pose_msg = new ROSLIB.Message({
        header: {
          frame_id: "map"
        },
        pose: {
          pose: {
            position: positionVec3,
            orientation: orientation
          }
        }
      });
      // send the goal
      InitialPoseClient.publish(pose_msg);
      console.log("Initial Pose sent:", pose_msg);
    }
  };

  this.rootObject.addEventListener("stagemousedown", function (event) {
    mouseEventHandler(event, "down");
  });

  this.rootObject.addEventListener("stagemousemove", function (event) {
    mouseEventHandler(event, "move");
  });

  this.rootObject.addEventListener("stagemouseup", function (event) {
    mouseEventHandler(event, "up");
  });
};

NAV2D.InitialPose.prototype.destroy = function () {
  this.rootObject.removeAllEventListeners();
};

NAV2D.Navigator.prototype.destroy = function () {
  this.rootObject.removeAllEventListeners();
};
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator.
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * topic (optional) - the map topic to listen to
 *   * rootObject (optional) - the root object to add this marker to
 *   * continuous (optional) - if the map should be continuously loaded (e.g., for SLAM)
 *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
 *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
 *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
 *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
 *   * viewer - the main viewer to render to
 */
NAV2D.OccupancyGridClientNav = function (options) {
  var that = this;
  options = options || {};
  this.ros = options.ros;
  var topic = options.topic || "/map";
  var continuous = options.continuous;
  this.serverName = options.serverName || "/move_base";
  this.actionName = options.actionName || "move_base_msgs/MoveBaseAction";
  this.rootObject = options.rootObject || new createjs.Container();
  this.viewer = options.viewer;
  this.withOrientation = options.withOrientation || false;
  // var init_pose = options.init_pose;
  // var goal_pose = options.goal_pose;
  this.navigator = null;
  this.initialPose = null;
  // setup a client to get the map
  var client = new ROS2D.OccupancyGridClient({
    ros: this.ros,
    rootObject: this.rootObject,
    continuous: continuous,
    topic: topic
  });
  client.on("change", function () {
    // that.initialPose = new NAV2D.InitialPose({
    //   ros: that.ros,
    //   rootObject: that.rootObject
    // });
    // that.navigator = new NAV2D.Navigator({
    //   ros: that.ros,
    //   serverName: that.serverName,
    //   actionName: that.actionName,
    //   rootObject: that.rootObject,
    //   withOrientation: that.withOrientation
    // });

    // scale the viewer to fit the map
    that.viewer.scaleToDimensions(
      client.currentGrid.width,
      client.currentGrid.height
    );
    that.viewer.shift(
      client.currentGrid.pose.position.x,
      client.currentGrid.pose.position.y
    );
  });
};
